#include "native_api.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>

int parse_and_exec_user_input_cmd(const char *cmd)
{
    // printf("输入指令:%s\n", cmd);
    if (strcmp(cmd, "stop") == 0)
    {
        MC_stop();
        return 0;
    }
    else if (strcmp(cmd, "param") == 0)
    {
        MC_print_params();
        return 0;
    }
    if (MC_is_running())
    {
        printf("电机运行中，无法执行该命令\n");
        return -1;
    }
    if (strncmp(cmd, "ol:", 3) == 0)
    {
        MC_set_open_loop(atoi(cmd + 3));
        return 0;
    }
    else if (strncmp(cmd, "old:", 4) == 0)
    {
        MC_set_open_loop_duty_cycle(strtod(cmd + 4, NULL));
        return 0;
    }
    else if (strncmp(cmd, "dir:", 4) == 0)
    {
        MC_set_motor_direction(atoi(cmd + 4));
        return 0;
    }
    else if (strncmp(cmd, "dur:", 4) == 0)
    {
        MC_set_duration(strtod(cmd + 4, NULL));
        return 0;
    }
    else if (strncmp(cmd, "file:", 5) == 0)
    {
        MC_set_data_output_file(cmd + 5);
        return 0;
    }
    else if (strncmp(cmd, "p:", 2) == 0)
    {
        MC_set_Kp(strtod(cmd + 2, NULL));
        return 0;
    }
    else if (strncmp(cmd, "i:", 2) == 0)
    {
        MC_set_Ki(strtod(cmd + 2, NULL));
        return 0;
    }
    else if (strncmp(cmd, "d:", 2) == 0)
    {
        MC_set_Kd(strtod(cmd + 2, NULL));
        return 0;
    }
    else if (strncmp(cmd, "target:", 7) == 0)
    {
        MC_set_target_speed(strtod(cmd + 7, NULL));
        return 0;
    }
    else if (strncmp(cmd, "start", 5) == 0)
    {
        MC_start();
        return 0;
    }
    else
    {
        puts("未知指令");
        return -1;
    }
}

int main(int argc, char *argv[])
{
    MC_init();
    MC_set_motor_direction(0);
    // 设置PID控制器的参数，包括目标速度
    MC_set_target_speed(220.0);
    MC_set_Kp(0.197282666954605);
    MC_set_Ki(3.63388243633273);
    MC_set_Kd(0.00267760799658669);

    char cmd_buf[50];
    while (1)
    {
        // 获取一行用户的指令
        fgets(cmd_buf, sizeof(cmd_buf), stdin);
        // 去掉换行符
        size_t length = strlen(cmd_buf);
        if (length > 0 && cmd_buf[length - 1] == '\n')
            cmd_buf[length - 1] = '\0';
        parse_and_exec_user_input_cmd(cmd_buf);
    }
    return 0;
}